/*
 * TestAvoidObstacles.java
 *
 * Created on 12 de noviembre de 2007, 07:16 AM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Test;

import Behaviors.Arbitrators.VectorAdditionArbitrator;
import Behaviors.Schemas.Motor.MSAvoidObstacles;
import Behaviors.Schemas.Motor.MSGoToPoint;
import Behaviors.Schemas.Motor.MSNoise;
import Behaviors.Schemas.Perception.*;
import Behaviors.Schemas.Schema;
import Controllers.BehaviorBasedController;
import Robot.*;
import Robot.Sensors.*;
import Util.*;
import java.util.Vector;
/**
 *
 * @author alejo
 */
public class TestAvoidObstacles {
    
    /**
     * Creates a new instance of TestAvoidObstacles
     */
 public static void process() {
        
        
       
    
    //***Definicion Robot 1*************************************
    //Robot robotFollower=new Robot("127.1.1.1", 6665, null);
    Robot marlin=new Robot("192.168.1.50",6665,"Marlin",0);
    
    Robot camara=new Robot("127.0.0.1",6668,"Camara",0);
    /*
    Position2d positionFollower;
    try{
        positionFollower=new Position2d(robotFollower,"position1");
    }
    catch(Exception e){
        System.out.println("Error connecting to Player!");
        System.exit(1);
        return;
    }
    */
    //***Definicion Robot 3*************************************
    
    FusionPosition2d positionMarlin;
    try{
        positionMarlin=new FusionPosition2d(camara,marlin,"positionMarlin",0);
    }
    catch(Exception e){
        e.printStackTrace();
        return;
    }
    
    Blobfinder bf;
     try{
            bf = new Blobfinder(camara,"bf",0);
     }
        catch(Exception e){
            e.printStackTrace();
            return;
        }
    
    marlin.runThreaded(10,0);
    camara.runThreaded(10,0);

    
    Vector elements = new Vector(1,1);
    
   // positionMarlin.setSimulElements(elements);

        //while(!sonar.updatePoses());
        //Formacion formacion=new Formacion();
        //positionLeader.setInitialPosition(new Pose2d(-6.0f,-6.0f,1.57f));
        //positionFollower.setInitialPosition(new Pose2d(-6.0f,-6.0f,1.57f));
        Vector2d point=new Vector2d(0,0,Vector2d.MODE_XY);
        
        //System.out.println("PointAngle="+Float.toString(point.getTheta()));
        
        //positionMarlin.setNoise(true);
        
        PSDetectObstaclesBlobfinder detect = new PSDetectObstaclesBlobfinder("ObstBlob",positionMarlin,bf,10);
        
        MSGoToPoint goto2=new MSGoToPoint("goto2",positionMarlin,point,MSGoToPoint.MODE_EXPONENTIAL);
        MSAvoidObstacles avoid = new MSAvoidObstacles("avoid",detect,positionMarlin, MSAvoidObstacles.MODE_EXPONENTIAL);
        avoid.setExponentialDistance(50);
        avoid.setMaxRange(30);
        MSNoise noise = new MSNoise("noise");
        noise.setGain(0.0f);
        
        Schema[] schemaArray = {(Schema)goto2,(Schema)avoid,(Schema)noise};
        //System.out.println(schemaArray.length);
        //DetectFormation detectFormation=new DetectFormation(position1, positionCenter);
        //MaintainFormation mantainFormation=new MaintainFormation(detectFormation);
        VectorAdditionArbitrator arbitrator1=new VectorAdditionArbitrator(schemaArray,positionMarlin,false);
        
        BehaviorBasedController controller=new BehaviorBasedController("avoidObst",marlin,arbitrator1,10);
       
        
        Thread hilo=new Thread(controller);
        
        //arbitrator1.setDebugMode(true);
        hilo.start();
        
        
            
        /*positionLeader.setSpeed(0,-1);
        while(true)
        {
            
            try{
                Thread.sleep(100);
            }
            catch(Exception e){}
            
        }*/
    }
}

